Adding new Nav2 loopback simulator #4614
Conversation
Signed-off-by: Steve Macenski <[email protected]>
SteveMacenski
changed the title
Codecov ReportAll modified and coverable lines are covered by tests ✅ |
| self.info('Loopback simulator initialized') | ||
|
|
||
| def setupTimerCallback(self): | ||
| # Publish initial identity odom transform & laser scan to warm up system |
There was a problem hiding this comment.
Minor nit.
Does this comment match what is happening?
Do you mean for sendTransform() be called once only or repeatedly in the timer?
It would make sense if you also called sendTransform in the init().
You could just remove the comment from this setupTimerCallback()
There was a problem hiding this comment.
I mean for it to be repeated on a timer that runs before the initial pose is set. The 'real' simulators or hardware robots will have their odometry systems continuously publishing and that odom->base_link is needed by many node's lifecycle startup processes to make Nav2 fully initialize. We could publish it once, but that would be fragile in case there's a delay or a long plugin initialization cycle for that message to be out of the TF buffer duration. Plus, this is how "real" robots and simulators operate so its analogoous
The comment is mostly there for tutorial value for later on-lookers. I agree most of my comments are not necessary
| # Don't consider velocities before the initial pose is set | ||
| return | ||
| self.curr_cmd_vel = msg | ||
| self.curr_cmd_vel_time = self.get_clock().now() |
There was a problem hiding this comment.
A comparison between the time in the msg.header and curr_cmd_vel_time could be used to identify, warn about and/or reject old cmd_vel messages.
There was a problem hiding this comment.
There is no message header in Twist
| # Initialize transforms (map->odom as input pose, odom->base_link start from identity) | ||
| self.initial_pose = msg.pose.pose | ||
| self.t_map_to_odom.transform.translation.x = self.initial_pose.position.x | ||
| self.t_map_to_odom.transform.translation.y = self.initial_pose.position.y |
There was a problem hiding this comment.
poor z. Maybe explicitly zero it.
| t_map_to_base_link.child_frame_id = 'base_link' | ||
| t_map_to_base_link.transform.translation.x = self.initial_pose.position.x | ||
| t_map_to_base_link.transform.translation.y = self.initial_pose.position.y | ||
| t_map_to_base_link.transform.translation.z = self.initial_pose.position.z |
There was a problem hiding this comment.
Making analog to comment above
|
|
||
| def publishTransforms(self, map_to_odom, odom_to_base_link): | ||
| map_to_odom.header.stamp = \ | ||
| (self.get_clock().now() + Duration(seconds=self.update_dur)).to_msg() |
There was a problem hiding this comment.
Why are transforms published with a header.stamp in the "future" but Odometry (in publishOdometry()) is not?
There was a problem hiding this comment.
This is how all the global localizers work. We need the transformation from map->odom to slightly "lead" the system so that anyone that wants to transform map->XYZ frame gets that data available between global localization updates. Since that is a static transformation between updates, the time isn't really all that important (unlike the odom->base_link). If you look at Cartographer, AMCL, SLAM Toolbox, etc they all do this from the very beginning of ROS 1 as a known design pattern to prevent floods of those "extrapolation into the future" errors
There was a problem hiding this comment.
Thanks for the explanation.
We have just killed off an controller_server control loop issue that is probably related to this.
i.e "Control loop missed its desired rate of %.4f Hz. Current loop rate is %.4f Hz."
Despite configuring for rtprio and we were still getting this warning
I think MPPI is effectively hanging around waiting for odom updates. We were running MPPI at 50Hz and the two ros_localisation ekfs at 30Hz and nav_sat_transform at 30Hz. So while it works I have a feeling there is effectively a live lock underneath it all.
Do you, in your robots change the robot_localization transform_time_offset from the default of 0.0? https://github.com/cra-ros-pkg/robot_localization/blob/f6b28e0c5bcf98d161cb65b9d28d00ced77fe3ab/params/ekf.yaml#L20
Bumping the ekfs to 100Hz and leaving nav_sat_transform at 30Hz makes nav2_controller/MPPI and me very happy.
There was a problem hiding this comment.
I change transform_time_offset for the global EKF for GPS fusion only, but yeah I do use that parameter for this same reason!
| """ | ||
|
|
||
|
|
||
| def addYawToQuat(quaternion, yaw_to_add): |
There was a problem hiding this comment.
Feel free to ignore below. I know its 2D and simple.
You could make a quaternion from the yaw and then multiply it to the existing quaternion.
In c++ I would also give it a normalize()
It seems that tf_transformations does not expose a quaternion normalisation.
https://github.com/DLu/tf_transformations/blob/07d77223288c457cfe71f361257e9fdd350c6fd9/tf_transformations/__init__.py#L809
Underneath it uses transforms3d.quaternions.qmult - http://matthew-brett.github.io/transforms3d/reference/transforms3d.quaternions.html#transforms3d.quaternions.qmult
That does provide a transforms3d.quaternions.qnorm - http://matthew-brett.github.io/transforms3d/reference/transforms3d.quaternions.html#qnorm
There was a problem hiding this comment.
Good to know! I didn't previously see the quaternion_multiply method in that library to use https://github.com/DLu/tf_transformations/blob/main/tf_transformations/__init__.py#L809. The lack of transformation libraries built into TF for Python is quite irritating. Thanks @DLu for this work!
Will make that update
* adding Nav2 loopback sim Signed-off-by: Steve Macenski <[email protected]> * drop performance by half * lintin * Add multirobot usecase comment * fixing copy paste error * fixing review comments --------- Signed-off-by: Steve Macenski <[email protected]>
* Updated README Table once Jazzy jobs turn over (#4482) * add new Jazzy matrix * missing header * test toolg * retry * done! * trim * trim * fix OS[0] * shutdown services in destructor of `ClearCostmapService` (#4495) Signed-off-by: GoesM_server <[email protected]> Co-authored-by: GoesM_server <[email protected]> * adjusting backup speed to be more reasonable (#4501) * Adding Costmap filters to system tests and cleaning up old gazebo classic files (#4502) * removing old files * removing old refs to gazebo classic * porting test body * including in root * Dock panel (#4458) * Initial docking panel Signed-off-by: Alberto Tudela <[email protected]> * Only one goal status Signed-off-by: Alberto Tudela <[email protected]> * Added dock pose Signed-off-by: Alberto Tudela <[email protected]> * Fix size of text Signed-off-by: Alberto Tudela <[email protected]> * Update rviz Signed-off-by: Alberto Tudela <[email protected]> * Update rviz config Signed-off-by: Alberto Tudela <[email protected]> --------- Signed-off-by: Alberto Tudela <[email protected]> * Fix default view (#4504) * Fix logo in nav2 panel (#4505) * Fix logo in nav2 panel Signed-off-by: Alberto Tudela <[email protected]> * Move icon Signed-off-by: Alberto Tudela <[email protected]> --------- Signed-off-by: Alberto Tudela <[email protected]> * Fix world to map coordinate conversion (#4506) Signed-off-by: HovorunBh <[email protected]> * Update README.md Signed-off-by: Steve Macenski <[email protected]> * Add dock id (#4511) * Implement dock id Signed-off-by: redvinaa <[email protected]> * Update tests Signed-off-by: redvinaa <[email protected]> * Update docs Signed-off-by: redvinaa <[email protected]> * Fix virtual override error Signed-off-by: redvinaa <[email protected]> --------- Signed-off-by: redvinaa <[email protected]> * fix(nav2_costmap_2d): make obstacle layer not current on enabled toggle (#4507) Signed-off-by: Kemal Bektas <[email protected]> Co-authored-by: Kemal Bektas <[email protected]> * min_turning_r_ getting param fix (#4510) * min_turning_r_ getting param fix Signed-off-by: Ivan Radionov <[email protected]> * Update nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Ivan Radionov <[email protected]> --------- Signed-off-by: Ivan Radionov <[email protected]> Signed-off-by: Steve Macenski <[email protected]> Co-authored-by: Ivan Radionov <[email protected]> Co-authored-by: Steve Macenski <[email protected]> * fixing gz sim launch file by using gz directly (#4514) * port wait behavior to new gazebo (#4471) Signed-off-by: stevedan <[email protected]> Signed-off-by: Steve Macenski <[email protected]> Co-authored-by: Steve Macenski <[email protected]> * Completely rewritten spin, backup, and drive on heading tests to remove flakiness (#4515) * port backup behavior to new gazebo Signed-off-by: stevedan <[email protected]> * port drive on heading behavior to new gazebo Signed-off-by: stevedan <[email protected]> * completely rewritten spin test * lint * complete flaky test rewrite --------- Signed-off-by: stevedan <[email protected]> Signed-off-by: Steve Macenski <[email protected]> Co-authored-by: stevedan <[email protected]> * Return out of map update if frames mismatch. Signed-off-by Joey Yang (#4517) Signed-off-by: Joey Yang <[email protected]> * Fix error_code_id (#4522) Signed-off-by: redvinaa <[email protected]> * completely shutdown dyn_params_handler_ (s) (#4521) * completely shutdown dny_params_handler_ in nav2_amcl Signed-off-by: GoesM_server <[email protected]> * completely shutdown dyn_param_handler_ in controller_server Signed-off-by: goes <[email protected]> * compeletly shutdown dyn_params_handler in nav2_costmap_2d Signed-off-by: goes <[email protected]> * compeletly shutdown dyn_param_handler in nav2_docking Signed-off-by: goes <[email protected]> * completely shutdown dyn_params_handler in nav2_velocity_smoother Signed-off-by: goes <[email protected]> * compeletly shutdown dyn_param_handler in waypoint_follower Signed-off-by: goes <[email protected]> * typo fixed Signed-off-by: goes <[email protected]> * graceful-controller & dwb_controller Signed-off-by: goes <[email protected]> * mppi-controller Signed-off-by: goes <[email protected]> * navfn_planner & regulated_..controller Signed-off-by: goes <[email protected]> * rotation_..controller & samc_planners Signed-off-by: goes <[email protected]> * A*planner Signed-off-by: goes <[email protected]> * code style Signed-off-by: goes <[email protected]> * 1 Signed-off-by: goes <[email protected]> * fixed Signed-off-by: goes <[email protected]> * fix the usage of weak_ptr Signed-off-by: goes <[email protected]> * code-style Signed-off-by: goes <[email protected]> * weak_ptr released Signed-off-by: goes <[email protected]> * code style Signed-off-by: goes <[email protected]> * code style Signed-off-by: goes <[email protected]> * code style Signed-off-by: goes <[email protected]> * code style update Signed-off-by: goes <[email protected]> * back Signed-off-by: goes <[email protected]> * rebase conflict resovled Signed-off-by: goes <[email protected]> * rebase error fixed Signed-off-by: goes <[email protected]> * fixed2 Signed-off-by: goes <[email protected]> * rebase fixed 3 Signed-off-by: goes <[email protected]> * 33 Signed-off-by: goes <[email protected]> * shared_ptr into weak_ptr Signed-off-by: GoesM <[email protected]> * remove adundant node.resest() Signed-off-by: GoesM <[email protected]> --------- Signed-off-by: GoesM_server <[email protected]> Signed-off-by: goes <[email protected]> Signed-off-by: GoesM <[email protected]> Co-authored-by: GoesM_server <[email protected]> * check nullptr in smoothPlan() (#4544) * check nullptr in smoothPlan() Signed-off-by: GoesM <[email protected]> * code-style Signed-off-by: GoesM <[email protected]> * code-style Signed-off-by: GoesM <[email protected]> * simple change Signed-off-by: GoesM <[email protected]> --------- Signed-off-by: GoesM <[email protected]> Co-authored-by: GoesM <[email protected]> * check nullPtr in `computeControl()` (#4548) Signed-off-by: goes <[email protected]> Co-authored-by: goes <[email protected]> * Straight analytic expansions (#4549) * Add test to verify analytic expansions are straight Signed-off-by: James Ward <[email protected]> * Use continuous scaling of angle when performing analytic expansion Only applies to Hybrid A* - behaviour in lattice planner is unchanged Signed-off-by: James Ward <[email protected]> --------- Signed-off-by: James Ward <[email protected]> * Rviz tool to get cost of costmap cell (#4546) * Added GetCost srv Signed-off-by: Jatin Patil <[email protected]> * Added Service in costmap_2d Signed-off-by: Jatin Patil <[email protected]> * Added Rviz tool Signed-off-by: Jatin Patil <[email protected]> * Fixed Styling Signed-off-by: Jatin Patil <[email protected]> * Fixed Styles and Linting Signed-off-by: Jatin Patil <[email protected]> * Fixed Linting Signed-off-by: Jatin Patil <[email protected]> * Added Bool use_footprint to srv Signed-off-by: Jatin Patil <[email protected]> * Added unit test for costmap costcell cost service Signed-off-by: Jatin Patil <[email protected]> * Fixed unit test script Signed-off-by: Jatin Patil <[email protected]> * Added theta, Updated unit test, Updated rviz tool service call logic Signed-off-by: Jatin Patil <[email protected]> * Updated requested changes Signed-off-by: Jatin Patil <[email protected]> --------- Signed-off-by: Jatin Patil <[email protected]> * Switch to new-style static_transform_publisher arguments. (#4563) These arguments have been the preferred way to use things since at least Humble. This avoids warnings when running it for the tests. Signed-off-by: Chris Lalancette <[email protected]> * Updated slack link (#4565) Signed-off-by: Steve Macenski <[email protected]> * Update README.md (#4589) Signed-off-by: Steve Macenski <[email protected]> * fix flickering visualization (#4561) * Fix Flickering visualization Signed-off-by: Vladyslav Hrynchak <[email protected]> * Refactoring Costmap2DPublisher and Costmap2DROS Signed-off-by: Vladyslav Hrynchak <[email protected]> * Refactoring costmap_2d_ros.cpp Signed-off-by: Vladyslav Hrynchak <[email protected]> * Refactoring Costmap2DPublisher and Costmap2DROS Signed-off-by: Vladyslav Hrynchak <[email protected]> * Update costmap_2d_publisher.cpp Signed-off-by: Vladyslav Hrynchak <[email protected]> * Change map_vis_z from float to double Signed-off-by: Vladyslav Hrynchak <[email protected]> * Add comment to map_vis_z_ parameter Signed-off-by: Vladyslav Hrynchak <[email protected]> --------- Signed-off-by: Vladyslav Hrynchak <[email protected]> * Copy fix-terminate diff from opennav_docking repo (#4598) * Copy fix-terminate diff from opennav_docking repo Signed-off-by: redvinaa <[email protected]> * Lint Signed-off-by: redvinaa <[email protected]> --------- Signed-off-by: redvinaa <[email protected]> * Fix race condition in AMCL for #4537 (#4605) * Fixed timed_behavior.hpp (#4602) Signed-off-by: Vladyslav Hrynchak <[email protected]> * Adding new Nav2 loopback simulator (#4614) * adding Nav2 loopback sim Signed-off-by: Steve Macenski <[email protected]> * drop performance by half * lintin * Add multirobot usecase comment * fixing copy paste error * fixing review comments --------- Signed-off-by: Steve Macenski <[email protected]> * Added laser data from map in nav2_loopback_sim (#4617) * Added laser data from map Signed-off-by: Jatin Patil <[email protected]> * Fixed Linting Signed-off-by: Jatin Patil <[email protected]> * Fixed Linting Signed-off-by: Jatin Patil <[email protected]> * Fixed few requested changes Signed-off-by: Jatin Patil <[email protected]> * Fixed linting Signed-off-by: Jatin Patil <[email protected]> * Requested changes Signed-off-by: Jatin Patil <[email protected]> * Linting Signed-off-by: Jatin Patil <[email protected]> * Added parameters and fixed requested changes Signed-off-by: Jatin Patil <[email protected]> * linting Signed-off-by: Jatin Patil <[email protected]> * Added scan using LineIterator Signed-off-by: Jatin Patil <[email protected]> * LineIterator max_distance or range_max Signed-off-by: Jatin Patil <[email protected]> * min of max_distance or range_max Signed-off-by: Jatin Patil <[email protected]> * final updates working correctly Signed-off-by: Jatin Patil <[email protected]> * Fix lint Signed-off-by: Jatin Patil <[email protected]> * requested changes Signed-off-by: Jatin Patil <[email protected]> * Update nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py Signed-off-by: Steve Macenski <[email protected]> * Update nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: Jatin Patil <[email protected]> Signed-off-by: Steve Macenski <[email protected]> Co-authored-by: Steve Macenski <[email protected]> * Making base frame ID for map to base link transform based on base frame ID parameter (#4632) Signed-off-by: Steve Macenski <[email protected]> * Update Smac Planner for rounding to closest bin rather than flooring (#4636) * adding stamped option for loopbacks im (#4637) Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: GoesM_server <[email protected]> Signed-off-by: Alberto Tudela <[email protected]> Signed-off-by: HovorunBh <[email protected]> Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: redvinaa <[email protected]> Signed-off-by: Kemal Bektas <[email protected]> Signed-off-by: Ivan Radionov <[email protected]> Signed-off-by: stevedan <[email protected]> Signed-off-by: Joey Yang <[email protected]> Signed-off-by: goes <[email protected]> Signed-off-by: GoesM <[email protected]> Signed-off-by: James Ward <[email protected]> Signed-off-by: Jatin Patil <[email protected]> Signed-off-by: Chris Lalancette <[email protected]> Signed-off-by: Vladyslav Hrynchak <[email protected]> Co-authored-by: GoesM <[email protected]> Co-authored-by: GoesM_server <[email protected]> Co-authored-by: Alberto Tudela <[email protected]> Co-authored-by: Bohdan <[email protected]> Co-authored-by: Vince Reda <[email protected]> Co-authored-by: Kemal Bektas <[email protected]> Co-authored-by: Kemal Bektas <[email protected]> Co-authored-by: Ivan Radionov <[email protected]> Co-authored-by: Ivan Radionov <[email protected]> Co-authored-by: Stevedan Ogochukwu Omodolor <[email protected]> Co-authored-by: stevedan <[email protected]> Co-authored-by: Joey Yang <[email protected]> Co-authored-by: James Ward <[email protected]> Co-authored-by: Jatin Patil <[email protected]> Co-authored-by: Chris Lalancette <[email protected]> Co-authored-by: Vladyslav Hrynchak <[email protected]>
#3924